package ch.ethz.fcl.process.polygon;

import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;

import ch.ethz.fcl.Constant;
import ch.ethz.fcl.tools.Tools;

/**
 * line can be defined as two points or a point, an angle and a length
 * 
 * @author crazyfrog
 * 
 */
public class Line {
	Point p1, p2;

	Point center;
	double angleDegree = -1;
	double length;

	double prinAxis = -1;

	public Line(Point p1, Point p2) {
		this.p1 = p1;
		this.p2 = p2;

		center = Tools.center(p1, p2);

		double dx = p2.x - p1.x;
		double dy = p2.y - p1.y;

		angleDegree = Tools.radToDegree(Math.atan(dy / dx));
		// bound angle to (0, 180)
		angleDegree = (angleDegree + 180) % 180;

		length = Math.sqrt(dx * dx + dy * dy);
	}

	public Line(Point center, double angle, double length) {
		this.center = center;
		this.angleDegree = angle;
		this.length = length;

		double angleRad = Tools.degreeToRad(angle);

		double p1x = center.x + length / 2 * Math.cos(angleRad);
		double p2x = center.x - length / 2 * Math.cos(angleRad);

		double p1y = center.y + length / 2 * Math.sin(angleRad);
		double p2y = center.y - length / 2 * Math.sin(angleRad);

		p1 = new Point(p1x, p1y);
		p2 = new Point(p2x, p2y);
	}

	/**
	 * if two line are parallel and close enough to each other, we say they are
	 * in the same line
	 * 
	 * @param line2
	 *            another line
	 * @return null if they don't belong to same line, otherwise the same line
	 */
	public Line sameLine(Line line2) {
		if (!isParallel(line2))
			return null;

		if (distance(line2.centroid()) > Constant.MIN_LINE_LENGTH)
			return null;

		double dist = distance(line2);
		if (dist > Constant.MIN_LINE_LENGTH)
			return null;

		float p1MinDist = Math.min(Tools.dist(p1, line2.p1),
				Tools.dist(p1, line2.p2));
		float p2MinDist = Math.min(Tools.dist(p2, line2.p1),
				Tools.dist(p2, line2.p2));
		float maxDist = Math.max(p1MinDist, p2MinDist);
		float minDist = Math.min(p1MinDist, p2MinDist);

		if (maxDist / minDist < 5)
			return null;

		Point center = Tools.center(p1, p2, line2.p1, line2.p2);
		// two end points from center
		Point endP1 = p1, endP2 = p2;
		double distP1 = Tools.dist(p1, center);
		double distP2 = Tools.dist(p2, center);
		double distLine2P1 = Tools.dist(line2.p1, center);
		double distLine2P2 = Tools.dist(line2.p2, center);
		if (distP1 > distP2) {
			if (distLine2P1 > distLine2P2)
				endP2 = line2.p1;
			else
				endP2 = line2.p2;
		} else {
			if (distLine2P1 > distLine2P2)
				endP1 = line2.p1;
			else
				endP1 = line2.p2;
		}
		double angle = Tools.mergeDegree(angleDegree, length,
				line2.angleDegree, line2.length);
		double verticalAngle = (angle + 90) % 180;

		Line lineThroughCenter = new Line(center, angle, length);
		Line lineThroughEndP1 = new Line(endP1, verticalAngle, length);
		Line lineThroughEndP2 = new Line(endP2, verticalAngle, length);

		Point lineP1 = lineThroughCenter.intersect(lineThroughEndP1);
		Point lineP2 = lineThroughCenter.intersect(lineThroughEndP2);

		return new Line(lineP1, lineP2);
	}

	// check parallel with another line
	public boolean isParallel(Line line2) {
		double sub = Math.abs(angleDegree() - line2.angleDegree());
		// if parallel, between (0, thre) or (180 - thre, 180)
		if (sub < Constant.ANGLE_THRE || sub > (180 - Constant.ANGLE_THRE)) {
			return true;
		}
		return false;
	}

	// check parallel with an angle
	public boolean isParallel(double angleDegree) {
		double sub = Math.abs(angleDegree() - angleDegree);
		if (sub < Constant.ANGLE_THRE || sub > (180 - Constant.ANGLE_THRE)) {
			return true;
		}
		return false;
	}

	// return the minimum points distance between two lines
	public double distance(Line line2) {
		double minDist = Double.MAX_VALUE;

		minDist = Math.min(minDist, Tools.dist(p1, line2.p1));
		minDist = Math.min(minDist, Tools.dist(p2, line2.p1));
		minDist = Math.min(minDist, Tools.dist(p1, line2.p2));
		minDist = Math.min(minDist, Tools.dist(p2, line2.p2));

		return minDist;
	}

	// return distance between the line and a point
	public double distance(Point p) {
		double tan = Math.tan(Tools.degreeToRad(angleDegree));
		return Math.abs(tan * p.x - p.y + (center.y - center.x * tan))
				/ Math.sqrt(1 + tan * tan);
	}

	// return the intersect point with another line
	public Point intersect(Line line2) {
		if (isParallel(line2))
			return null;
		else {
			double tan0 = Math.tan(Tools.degreeToRad(angleDegree));
			double tan1 = Math.tan(Tools.degreeToRad(line2.angleDegree));

			Point center = centroid();
			Point line2Center = line2.centroid();
			double x = (line2Center.y - center.y - line2Center.x * tan1 + center.x
					* tan0)
					/ (tan0 - tan1);
			double y = (line2Center.y * tan0 - center.y * tan1 + (center.x - line2Center.x)
					* tan0 * tan1)
					/ (tan0 - tan1);

			return new Point(x, y);
		}
	}

	public Line getLineWithAngle(double angle) {
		double lineAngle = angle;
		if (!isParallel(angle))
			lineAngle = (angle + 90) % 180;

		return new Line(center, lineAngle, length);
	}

	public MatOfPoint toMatOfPoint() {
		return new MatOfPoint(p1, p2);
	}

	public float angleDegree() {
		return (float) angleDegree;
	}

	public float length() {
		return (float) length;
	}

	public Point centroid() {
		return center;
	}
}
